﻿using System;
using System.Collections.Generic;
using System.Windows.Forms;
using System.Linq;
using System.Drawing.Drawing2D;
using System.Drawing;
using System.Text;
using Aldebaran.Proxies;

namespace NAO_Image_Processing
{
    public class NAOControl
    {
        /*statische Variablen*/
        private static double camYAngleOffset = 1.2;
        /*Kontrollvariablen*/
        public bool DEBUG = false;
        public bool DEBUG_PICTURE_MAKER = false;
        string _naoIP;
        int _naoPort;
        bool _naoConnected = false;
        /*Kamera Variablen*/
        double camAnglePitch, camAnglePitchWorld, camAngleYaw, camHeight, camXOffset;
        List<float> camPosition;
        int resolutionWidth, resolutionHeight;
        int fps;
        
        /*Variablen zur Entfernungsmessung*/
        static double[, ,] entfKreisboegen = new double[10, 30, 2];
        static double[, ,] posBildflaeche = new double[10, 30, 2];
        static double[, ,] posBildflaechePixel = new double[10, 30, 2];
        static Matrix matZ = new Matrix(3, 3);
        static Matrix matN = new Matrix(3, 3);
        static Matrix P = new Matrix(3, 1);
        static Matrix g1 = new Matrix(3, 1);
        static Matrix g2 = new Matrix(3, 1);
        static Matrix g3 = new Matrix(3, 1);
        static Matrix m = new Matrix(3, 1);
        static Matrix n = new Matrix(3, 1);
        static Matrix o = new Matrix(3, 1);
        static Matrix p = new Matrix(3, 1);
        static Matrix q = new Matrix(3, 1);
        static Matrix b1 = new Matrix(3, 1);
        static Matrix b2 = new Matrix(3, 1);
        static double r, s, t;
        static double[, ,] entfernungenFB = new double[10,1,1];
        
        /*Proxies definieren*/
        public MotionProxy _motionProxy;
        public RobotPostureProxy _robotPostureProxy;
        public BatteryProxy _batteryProxy;
        public MemoryProxy _memoryProxy;
        public BehaviorManagerProxy _behaviourManagerProxy;

        public NAOControl(string ip = "127.0.0.1", int port = 9559)
        {
            _naoIP = ip;
            _naoPort = port;
            _naoConnected = true;
            try
            {
                _motionProxy = new MotionProxy(_naoIP, _naoPort);
                _memoryProxy = new MemoryProxy(_naoIP, _naoPort);
                _behaviourManagerProxy = new BehaviorManagerProxy(_naoIP, _naoPort);
                if (!DEBUG)
                {
                    _batteryProxy = new BatteryProxy(_naoIP, _naoPort);
                    _robotPostureProxy = new RobotPostureProxy(_naoIP, _naoPort);
                }
            }
            catch (Exception ex)
            {
                Console.Error.WriteLine("Error, creating NAO-Proxies: " + ex.Message);
            }
        }

        /*           /| b_1 
         *          / |  
         *         /al|
         *        /   | 
         *       /    |
         *      /-----| b_2
         *     /|\    |
         * c  /a| \   | 
         *   /  |  \ga| 
         *  /   |   \ |
         * /be  | ga2\|
         * -----------------
         * a_c  a_x
         * b=motionProxy.getPosition("CameraTop", 1, true)[2]*100 --> Höhe des Kopfes über dem Boden in cm
         * a=b*tan(alpha)                               Z
         * c=sqrt(a^2+b^2)                              |   
         * ################                             |    
         * Kameraversatz in Y-Richtung = 1.2°          / \
         * Horizontaler Blickwinkel = 60.9°           /   \
         * Vertikaler Blickwinkel = 47.64°           /     \
         * Head-Yaw [Z] = 239° [-119.5° - 119.5°]   X       Y
         * Head-Pitch [X] = 68° [-38.5° - 29.5°]
         */
        public void berechneModell()
        {
            try
            {
                /* Hardware-Variablen */
                camAnglePitch = (Convert.ToDouble(_memoryProxy.getData("Device/SubDeviceList/HeadPitch/Position/Actuator/Value")) * 180 / Math.PI);
                camAngleYaw = (Convert.ToDouble(_memoryProxy.getData("Device/SubDeviceList/HeadYaw/Position/Actuator/Value")) * 180 / Math.PI);
            }
            catch (Exception ex)
            {
                System.Console.Error.WriteLine("SOAP-Error in brechneModell() while getting Head-Pitch and Head-Yaw." + ex.Message);
            }
            camAnglePitchWorld = 90 - camYAngleOffset - camAnglePitch;
            /* Kamera Position ermitteln 
             * [0] = X; [1] = Y; [2] = Z
             */
            try
            {
                camPosition = _motionProxy.getPosition("CameraTop", 1, true);
            }
            catch (Exception ex)
            {
                System.Console.Error.WriteLine("SOAP-Error in berechneModell() while getting Camera-Position." + ex.Message);
            }
            camXOffset = camPosition[0]; //Offset der Kamera in X-Richtung
            camHeight = camPosition[2]; //Höhe der Kamera

            /* Winkel berechnen */
            double alpha = camAnglePitchWorld;
            double beta = 90 - alpha; //

            /* Abstände bestimmen */
            double a = camHeight * Math.Tan(alpha * Math.PI / 180) - camXOffset; //Abstand Fuß/Boden
            double c = Math.Sqrt(Math.Pow(a, 2) + Math.Pow(camHeight, 2)); //Abstand Kamera/Boden
            
            /* Werte runden und zurückgeben
             * [0] = Fuß/Boden
             * [1] = Höhe der Kamera
             * [2] = Kamera/Boden
             */
            List<double> retKinematic = new List<double>();
            retKinematic.Add(Math.Round(a * 100, 2)); retKinematic.Add(Math.Round(camHeight * 100, 2)); retKinematic.Add(Math.Round(c * 100, 2));
        }

        public List<double[, ,]> berechneEntfernungen()
        {
            List<double[, ,]> returnValues = new List<double[, ,]>();
            List<double> returnDistancesA = new List<double>();
            List<double> returnHeights = new List<double>();
            double lowerAngle = 23.82;
            berechneModell();

            /* Berechnung der Fuß/Boden Entfernungen für die Head-Pitch-Winkel
             */
            int counter = 0;
            for (double i = camAnglePitchWorld; i > camAnglePitchWorld-lowerAngle; i -= 2.5)
            {
                double a = camHeight * Math.Tan(i * Math.PI / 180) - camXOffset;
                returnDistancesA.Add(a);
                entfernungenFB[counter, 0, 0] = a*100;
                counter++;
            }
            
            /* Berechnung der Kreispunkte auf dem Boden zu den jeweiligen Fuß/Boden Entfernugnen
             */
            Matrix tmp = new Matrix(3,1);
            for (int i = 0; i < 10; i++)
            {
                for(int j=0;j<30;j++)
                {
                    /*xy-Position*/
                    entfKreisboegen[i, j, 0] = Math.Round(returnDistancesA[i]*100 * Math.Cos(deg2rad(j*2) + (deg2rad(270))),4);
                    entfKreisboegen[i, j, 1] = -Math.Round(returnDistancesA[i]*100 * Math.Sin(deg2rad(j*2) + (deg2rad(270))), 4);
                    /* Berechnung der Position der Kreispunkte auf der Bildfläche */
                    tmp = berechnePositionAufBildflaeche(returnDistancesA[9]*100, camHeight*100, entfKreisboegen[i, j, 1], entfKreisboegen[i, j, 0]);
                    posBildflaeche[i, j, 0] = Math.Round(tmp[2, 0], 4);
                    posBildflaeche[i, j, 1] = Math.Round(tmp[1, 0], 4);
                }
            }

            /* returnValues[0] = Position der Kreispunkte auf der Bildfläche
             * returnValues[1] = Fuß/Boden Entfernungen
             */
            returnValues.Add(posBildflaeche);
            returnValues.Add(entfernungenFB);
            return returnValues;
        }

        public Matrix berechnePositionAufBildflaeche(double d, double h, double x, double z)
        {
            /* Berechnung der Position auf der Bildfläche */
            /*Geradengleichung*/
            P[0, 0] = x; P[1, 0] = 0; P[2, 0] = z;
            m[0, 0] = 0; m[1, 0] = h; m[2, 0] = 0;
            n = m - P;

            /*Ebenengleichung*/
            g1[0, 0] = d; g1[1, 0] = 0; g1[2, 0] = -200;
            g2[0, 0] = d; g2[1, 0] = 500; g2[2, 0] = -200;
            g3[0, 0] = d; g3[1, 0] = 0; g3[2, 0] = 200;
            o = g1;
            p = g1 - g2;
            q = g3 - g1;

            /*Parameterberechnung*/
            matN[0, 0] = n[0, 0]; matN[0, 1] = -p[0, 0]; matN[0, 2] = -q[0, 0];
            matN[1, 0] = n[1, 0]; matN[1, 1] = -p[1, 0]; matN[1, 2] = -q[1, 0];
            matN[2, 0] = n[2, 0]; matN[2, 1] = -p[2, 0]; matN[2, 2] = -q[2, 0];
            /*r*/
            matZ[0, 0] = o[0, 0] - m[0, 0]; matZ[0, 1] = -p[0, 0]; matZ[0, 2] = -q[0, 0];
            matZ[1, 0] = o[1, 0] - m[1, 0]; matZ[1, 1] = -p[1, 0]; matZ[1, 2] = -q[1, 0];
            matZ[2, 0] = o[2, 0] - m[2, 0]; matZ[2, 1] = -p[2, 0]; matZ[2, 2] = -q[2, 0];
            r = matZ.Determinante() / matN.Determinante();
            matZ = null;
            matZ = new Matrix(3, 3);
            /*s*/
            matZ[0, 0] = n[0, 0]; matZ[0, 1] = o[0, 0] - m[0, 0]; matZ[0, 2] = -q[0, 0];
            matZ[1, 0] = n[1, 0]; matZ[1, 1] = o[1, 0] - m[1, 0]; matZ[1, 2] = -q[1, 0];
            matZ[2, 0] = n[2, 0]; matZ[2, 1] = o[2, 0] - m[2, 0]; matZ[2, 2] = -q[2, 0];
            s = matZ.Determinante() / matN.Determinante();
            matZ = null;
            matZ = new Matrix(3, 3);
            /*t*/
            matZ[0, 0] = n[0, 0]; matZ[0, 1] = -p[0, 0]; matZ[0, 2] = o[0, 0] - m[0, 0];
            matZ[1, 0] = n[1, 0]; matZ[1, 1] = -p[1, 0]; matZ[1, 2] = o[1, 0] - m[1, 0];
            matZ[2, 0] = n[2, 0]; matZ[2, 1] = -p[2, 0]; matZ[2, 2] = o[2, 0] - m[2, 0];
            t = matZ.Determinante() / matN.Determinante();
            matZ = null;
            matZ = new Matrix(3, 3);

            /*Gleichungen lösen*/
            /*Geradengleichung*/
            b1 = m + r * n;
            /*Ebenengleichung*/
            b2 = o + (s * p) + (t * q);
            return b1;
        }

        public double deg2rad(double deg)
        {
            return deg * Math.PI / 180;
        }

        public double rad2deg(double rad)
        {
            return rad * 180 / Math.PI;

        }

        public static double euklidDistanz(double x, double y)
        {
            return Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
        }

        public double euklidDistanz(List<double[]> punkte)
        {
            double sum = 0;
            try
            {
                sum = Math.Pow(punkte[0][0] - punkte[1][0], 2) + Math.Pow(punkte[0][1] - punkte[1][1], 2);
            }
            catch (ArgumentOutOfRangeException ex)
            {
                Console.Error.WriteLine(ex.Message);
            }
            return Math.Sqrt(sum);
        }

        public void setConnectedStatus(bool status)
        {
            _naoConnected = status;
        }

        public bool isConnected()
        {
            return _naoConnected;
        }

        public string getIP()
        {
            return _naoIP;
        }

        public int getPort()
        {
            return _naoPort;
        }

        ///<summary>
        /// Gibt die Höhe der Kamera über dem Boden in cm an
        /// </summary>
        /// 
        /// <param name="camHeigth">Höhe in cm </param>
        public double getCamHeigth()
        {
            return camHeight*100;
        }

        public double getCamAnglePitch()
        {
            return camAnglePitch;
        }

        public double getCamAngleYaw()
        {
            return camAngleYaw;
        }

        ///<summary>
        /// Gibt die Auflösung zurück
        /// [0] -> Breite
        /// [1] -> Höhe
        /// </summary>
        /// 
        /// <param name="resolutionWidth">Breite in Pixel</param>
        /// <param name="resolutionHeight">Hoehe in Pixel</param>
        public int[] getResolution()
        {
            return new int[] { resolutionWidth, resolutionHeight };
        }

        ///<summary>
        /// Setzt die Auflösung
        /// [0] -> Breite
        /// [1] -> Höhe
        /// </summary>
        /// 
        /// <param name="resolution">Auflösung in Pixel</param>
        public void setResolution(int[] resolution)
        {
            resolutionWidth = resolution[0];
            resolutionHeight = resolution[1];
        }

        public int getFPS()
        {
            return fps;
        }

        public void setFPS(int fps)
        {
            this.fps = fps;
        }

        public bool isRobotPostureAvailable()
        {
            if (_robotPostureProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool isMotionAvailable()
        {
            if (_motionProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        
        public bool isBatteryAvailable()
        {
            if (_batteryProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool isMemoryAvailable()
        {
            if (_memoryProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        public bool releaseProxies()
        {
            try
            {
                _motionProxy = null;
                _robotPostureProxy = null;
                _batteryProxy = null;
                _memoryProxy = null;
                return true;
            }
            catch (Exception e)
            {
                Console.WriteLine("Exception caught, while disposing Proxies: " + e.Message);
                return false;
            }
        }

        ~NAOControl()
        {
            try
            {
                _motionProxy = null;
                _robotPostureProxy = null;
                _batteryProxy = null;
                _memoryProxy = null;
            }
            catch (Exception e)
            {
                Console.WriteLine("Exception caught, while disposing Proxies: " + e.Message);
            }
        }
    }
}
